﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
#if XNA
using Microsoft.Xna.Framework;
#else
using SilverArcade.SilverSprite;
#endif

namespace CakeAnimation
{
    public class IKSolver
    {
        public Bone Tip;
        public Vector2 Target;

        public IKSolver()
        {

        }

        public void SetTip(Bone tip)
        {
            Tip = tip;            
        }

        public void Update(AnimationContext context)
        {
            float weight = CalcDistance() * 0.3f;
            Bone bone = Tip;
            while (bone != null && !bone.Fixed)
            {
                float gradient = GetGradient(bone);

                bone.LocalRotation -= gradient * context.DeltaTime * weight;

                weight *= 0.25f;
                bone = bone.Parent;
            }

        }

        private float GetGradient(Bone bone)
        {
            Vector2 ToTip = Tip.GetTip() - bone.GetJointPosition();
            if (ToTip == Vector2.Zero)
            {
                return 0.0f;
            }

            Vector2 ToTarget = Target  - Tip.GetTip();

            Vector2 movement_vector = new Vector2(ToTip.Y, -ToTip.X);

            movement_vector.Normalize();
            ToTarget.Normalize();
            float gradient = Vector2.Dot(movement_vector, ToTarget);
            return gradient;
        }

        public Bone GetRoot()
        {
            Bone root = Tip;
            while (root.Parent != null)
            {
                root = root.Parent;
            }

            return root;
        }

        public float CalcDistance()
        {
            return (Tip.GetTip() - Target).Length();
        }
    }
}
